#include "k_pid.h"


void PID_init(void)
{

    
}


void PID_reset(_PID_Struct_t *pid_in)
{
    pid_in->last_err = 0;
    pid_in->out = 0;
}

void inc_PID_update(_PID_Struct_t *pid_in, s16 err)
{

    pid_in->out = pid_in->ki * err;

    if(err < pid_in->max_err && err > pid_in->min_err){
        //误差过大 不计算比例项
        pid_in->out += (err - pid_in->last_err) * pid_in->kp;
    }

    pid_in->last_err = err;
    pid_in->out >>= 8;

    if(pid_in->out > pid_in->max_output){
        pid_in->out = pid_in->max_output;
    }else if(pid_in->out < pid_in->min_output){
        pid_in->out = pid_in->min_output;
    }
}

